{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "0",
   "metadata": {},
   "source": [
    "# Arm and Gripper"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1",
   "metadata": {},
   "source": [
    "In the previous tutorial, we explored the concept of *goto* and basic control. Now, let's delve deeper into what we can do with Reachy's arms and grippers."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2",
   "metadata": {},
   "source": [
    "## Initialize Your Robot"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3",
   "metadata": {},
   "source": [
    "First, connect to your robot:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4",
   "metadata": {},
   "outputs": [],
   "source": [
    "from reachy2_sdk import ReachySDK\n",
    "import time\n",
    "\n",
    "reachy = ReachySDK(host='localhost')  # Replace with the actual IP address of your robot"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5",
   "metadata": {},
   "source": [
    "Next, we need to turn on the parts we want to use:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.l_arm.turn_on()\n",
    "reachy.r_arm.turn_on()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7",
   "metadata": {},
   "source": [
    "Since the grippers are part of the arms, they will also be turned on. You could, of course, turn on the whole robot by calling `reachy.turn_on()` directly.\n",
    "Let's check if the arms are on:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8",
   "metadata": {},
   "outputs": [],
   "source": [
    "print(reachy.r_arm.is_on())\n",
    "print(reachy.l_arm.is_on())"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9",
   "metadata": {},
   "source": [
    "## Control the arms\n",
    "\n",
    "Arms can be controlled in two spaces:\n",
    "\n",
    "* the **joint space**, which allows to read and write directly the angle values of each joint of the arm\n",
    "* the **cartesian space**, which consists of controlling the end effector's position and orientation in Reachy's coordinate system\n",
    "\n",
    "> Both spaces are quite different, and **we advise not to mix them** if you are not familiar with the output.\n",
    "In fact, values of the joint space are expressed in each actuator's coordinate system (respectively shoulder, elbow and wrist), whereas commands in cartesian space are expressed in Reachy's coordinate system"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "10",
   "metadata": {},
   "source": [
    "### Joint space goal"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "11",
   "metadata": {},
   "source": [
    "Reachy's arm offers 7 degrees of freedom. It also gives access to one joint for the gripper.\n",
    "The arm is divided as follows:\n",
    "- shoulder, composed of 2 joints (pitch and roll)\n",
    "- elbow, composed of 2 joints (yaw and pitch)\n",
    "- wrist, composed of 3 joints (roll, pitch and yaw)\n",
    "- gripper, composed of 1 joint (directly accessed at gripper level)\n",
    "\n",
    "We refer to the shoulder, elbow and wrist as actuators.\n",
    "For some actions, such as changing the compliance, it is the lowest level of control you will have.\n",
    "\n",
    "You can inspect the details of the arm with:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "12",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.r_arm.joints"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "13",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.l_arm.joints"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "14",
   "metadata": {},
   "source": [
    "You can easily access the position of each joint, **excluding the gripper**, in one call with `get_current_positions()`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "15",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.r_arm.get_current_positions()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "16",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.l_arm.get_current_positions()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "17",
   "metadata": {},
   "source": [
    "#### Move the arms in joint space"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "44d85230",
   "metadata": {},
   "source": [
    "> **The gripper's movements are always handled separately from the other joints of the arm.**"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "18",
   "metadata": {},
   "source": [
    "The simplest way to move an arm is to set the angle of each joint (excluding the gripper). Define a joint positions list:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "19",
   "metadata": {},
   "outputs": [],
   "source": [
    "r_elbow_at_90_deg = [0, 10, -15, -90, 0, 0, -5]\n",
    "l_elbow_at_90_deg = [0, -10, 15, -90, 0, 0, 5]"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "20",
   "metadata": {},
   "source": [
    "Send the joint goal positions to the arm with `goto()`"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "21",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.r_arm.goto(r_elbow_at_90_deg)\n",
    "reachy.l_arm.goto(l_elbow_at_90_deg, wait = True)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "22",
   "metadata": {},
   "source": [
    "In order to easily move a single joint, we can use the `goto()` method on a given joint:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "23",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.r_arm.elbow.pitch.goto(0)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "24",
   "metadata": {},
   "source": [
    "This method works exactly like a goto on the part. All gotos sent to joints are stacked on the part they belong to.\n",
    "For example:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "25",
   "metadata": {},
   "outputs": [],
   "source": [
    "part_goto_id = reachy.r_arm.goto([0, 10, -15, -90, 0, 0, -5])\n",
    "joint_goto_id = reachy.r_arm.elbow.pitch.goto(0)\n",
    "\n",
    "print(part_goto_id)\n",
    "print(joint_goto_id)\n",
    "\n",
    "print(reachy.r_arm.get_goto_queue())"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "26",
   "metadata": {},
   "source": [
    "`reachy.r_arm.elbow.pitch.goto(0)` command is stacked in r_arm goto queue.\n",
    "The goto on the elbow will be played when the previous command sent using `goto()` is over."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "27",
   "metadata": {},
   "source": [
    "The arms should have moved in a way similar to what we saw in the [goto tutorial](2_goto_introduction.ipynb). You already know that you can specify the duration or the interpolation mode of this kind of movement.\n",
    "\n",
    "We've only seen movements expressed in joint space, i.e., defined by a set of angles. How can we know the position of the gripper in space, or how can we reach an object for which we know its position? That's where kinematics come in."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "28",
   "metadata": {},
   "source": [
    "### Kinematics"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "29",
   "metadata": {},
   "source": [
    "The kinematic model describes the motion of a robot in mathematical form without considering the forces and torque affecting it. It only focuses on the geometric relationship between elements.\n",
    "\n",
    "We have defined the whole kinematic model of the arm. This means the translation and rotation required to go from one joint to the next one. \n",
    "\n",
    "[Long story](https://pollen-robotics.github.io/reachy2-docs/developing-with-reachy-2/basics/4-use-arm-kinematics/) short, there are two types of kinematics:\n",
    "- Forward kinematics: from the joint positions, the gripper pose is computed in cartesian space\n",
    "- Inverse kinematics: from a given gripper pose to reach, all joint positions are computed in joint space\n",
    "\n",
    "> You can easily use forward kinematics and inverse kinematics to switch respectively from joint space to cartesian space and from cartesian space to joint space."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "30",
   "metadata": {},
   "source": [
    "#### Forward Kinematics"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "31",
   "metadata": {},
   "source": [
    "Each arm has a `forward_kinematics` method that computes a 4x4 pose matrix (position and orientation of the gripper in space). For instance, the previous movement left the left elbow at 90°. The position (x, y, z) of the gripper is:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "32",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.l_arm.forward_kinematics()[:3, 3]"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "33",
   "metadata": {},
   "source": [
    "It is not mandatory to move the arm to compute forward kinematics. This can be done for any set of joint positions such as:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "34",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.l_arm.forward_kinematics([10, 0, 0, -90, 0, 0, 0])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "35",
   "metadata": {},
   "source": [
    "Reachy didn't move, but you know where it would have gone with an additional 10° applied to the shoulder joint."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "36",
   "metadata": {},
   "source": [
    "#### Inverse Kinematics"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "37",
   "metadata": {},
   "source": [
    "Inverse kinematics works in the opposite way. Let's say you want to reach an object for which you know its position. What would be the set of joint positions to provide to `goto`?"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "38",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "target = np.identity(4)\n",
    "target = np.array([[0, 0, -1, 0.3],\n",
    "    [0, 1, 0, 0.1],\n",
    "    [1, 0, 0, -0.3],\n",
    "    [0, 0, 0, 1]])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "39",
   "metadata": {},
   "outputs": [],
   "source": [
    "joint_positions = reachy.l_arm.inverse_kinematics(target)\n",
    "joint_positions"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "40",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.l_arm.goto(joint_positions)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "41",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.l_arm.goto_posture(\"default\")\n",
    "reachy.l_arm.goto(target, wait = True)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "42",
   "metadata": {},
   "source": [
    "> All these goto are illustrated in [draw_square](draw_square.py). Check it out to see how to make Reachy draw a square with its right arm!"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "43",
   "metadata": {},
   "source": [
    "### Cartesian space goal"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "44",
   "metadata": {},
   "source": [
    "Controlling the arm in cartesian space allows you to control the position of the gripper in Reachy's coordinate system. It is the recommended way to control the robot for grasping goals.  \n",
    "Let's go back to the *elbow_90* pose:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "45",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.l_arm.goto_posture('elbow_90', wait = True)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "46",
   "metadata": {},
   "source": [
    "You can easily access the current pose of the gripper using the previously seen method `forward_kinematics()`"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "47",
   "metadata": {},
   "outputs": [],
   "source": [
    "current_pose = reachy.l_arm.forward_kinematics()\n",
    "print(current_pose)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "48",
   "metadata": {},
   "source": [
    "#### Move the arms in cartesian space"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "49",
   "metadata": {},
   "source": [
    "To control the arm in cartesian space, use the `goto(...)` method.  \n",
    "Let's use it on the current_pose:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "50",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.l_arm.goto(current_pose)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "51",
   "metadata": {},
   "source": [
    "As you have just seen, the arm moved while the gripper remains in the exact same position and orientation: that's because the computed inverse kinematics solution is different from the joint positions we chose in joint space (`goto_posture()` is in fact a joint space method).  \n",
    "\n",
    "Let's send the arm to a new goal pose.  \n",
    "We need to define a 4x4 pose matrix as the new goal pose for the gripper:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "52",
   "metadata": {},
   "outputs": [],
   "source": [
    "new_pose = current_pose.copy()\n",
    "new_pose[0, 3] += 0.1\n",
    "print(new_pose)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "53",
   "metadata": {},
   "source": [
    "This new_pose is translated 10cm forward in Reachy's coordinate system (+10cm on Reachy's x axis).  \n",
    "You can send it to the robot:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "54",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.l_arm.goto(new_pose)\n",
    "reachy.l_arm.goto_posture()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "55",
   "metadata": {},
   "source": [
    "To simplify your life, you have access to functions to easily compute translation or rotation.  \n",
    "But first, have a look at the interpolation space!"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "884dfc68",
   "metadata": {},
   "source": [
    "### Interpolation space"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7a4f0d79",
   "metadata": {},
   "source": [
    "Let's make things a little more complicated now.  \n",
    "Sending a command in a space (joint or cartesian) does not mean the interpolation to reach the goal position is made in the same space.  \n",
    "\n",
    "***What does that mean?***  \n",
    "By default, the interpolation (all the intermediate positions the arm is going to take before reaching the goal position) is made in joint space: if the initial position of the `r_arm.shoulder.pitch` is `30` and we want to reach a position so that `r_arm.shoulder.pitch` is `15`, the goto will calculate the interpolation from 30 to 15 for the joint, **even if the goal position was given in cartesian_space**. The movement will be executed based solely on joint interpolation, without ensuring a smooth path of the end-effector in cartesian space.\n",
    "\n",
    "> **Default interpolation space is `joint_space`**"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0572d110",
   "metadata": {},
   "source": [
    "Here is an example to go through this.  \n",
    "From the `elbow_90` posture, we calculate a new pose that is 20cm ahead:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ac548ed0",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from reachy2_sdk.utils.utils import get_pose_matrix\n",
    "\n",
    "initial_pose = get_pose_matrix([0.25, -0.3, -0.3], [0, -90, 0])\n",
    "modified_pose = np.copy(initial_pose)\n",
    "modified_pose[0, 3] += 0.25"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "81b47732",
   "metadata": {},
   "source": [
    "We send the arm to the `elbow_90` posture first:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "132db3d7",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.r_arm.goto(initial_pose)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2a5f57e9",
   "metadata": {},
   "source": [
    "Then we send the arm to the new position, 20cm ahead. Watch carefully the trajectory of the gripper when moving to the new goal position:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "95fed794",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.r_arm.goto(modified_pose)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "cec15f4e",
   "metadata": {},
   "source": [
    "The gripper is going slightly down before reaching the target. This is because the movement was interpolated in joint space.  \n",
    "\n",
    "Put back the robot in the initial_pose, and set the `interpolation_space` argument to `cartesian_space` to reach the modified goal:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "07c84f62",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.r_arm.goto(initial_pose)\n",
    "reachy.r_arm.goto(modified_pose, interpolation_space=\"cartesian_space\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d4875d8f",
   "metadata": {},
   "source": [
    "There are 3 interpolation modes: linear, minimum_jerk and elliptical. minimum_jerk is the smoothest, so it's the default mode. However, if you want to chain movements without stopping inbetween, the linear mode is probably your best bet. The elliptical interpolation allows for curved trajectories. Let's visualize the different modes in action:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3481b20a",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.r_arm.goto(initial_pose, interpolation_space=\"cartesian_space\", interpolation_mode=\"linear\")\n",
    "reachy.r_arm.goto(modified_pose, interpolation_space=\"cartesian_space\", interpolation_mode=\"linear\")\n",
    "\n",
    "reachy.r_arm.goto(initial_pose, interpolation_space=\"cartesian_space\", interpolation_mode=\"minimum_jerk\")\n",
    "reachy.r_arm.goto(modified_pose, interpolation_space=\"cartesian_space\", interpolation_mode=\"minimum_jerk\")\n",
    "\n",
    "reachy.r_arm.goto(initial_pose, interpolation_space=\"cartesian_space\", interpolation_mode=\"elliptical\", arc_direction=\"above\")\n",
    "reachy.r_arm.goto(modified_pose, interpolation_space=\"cartesian_space\", interpolation_mode=\"elliptical\", arc_direction=\"above\")\n",
    "\n",
    "\n",
    "reachy.r_arm.goto(initial_pose, interpolation_space=\"cartesian_space\", interpolation_mode=\"elliptical\", arc_direction=\"right\")\n",
    "reachy.r_arm.goto(modified_pose, interpolation_space=\"cartesian_space\", interpolation_mode=\"elliptical\", arc_direction=\"right\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f99a2c13",
   "metadata": {},
   "source": [
    "The gripper is not following the same trajectory as previously, as the interpolation is now made in cartesian space."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0a296552",
   "metadata": {},
   "source": [
    "### Relative moves: translate_by / rotate_by"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9bb293a9",
   "metadata": {},
   "source": [
    "The `translate_by` and `rotate_by` methods are here to simplify simple translations and rotations. \n",
    "\n",
    "Note that if you use the `translate_by` method, which is a goto-based method, the interpolation space will be set by default to cartesian_space."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "323cf49c",
   "metadata": {},
   "source": [
    "> **Default interpolation space for translate_by is `cartesian_space`**"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1e7bbced",
   "metadata": {},
   "source": [
    "Use the `translate_by(...)` method to send the gripper back to the previous pose, asking for a translation 10cm back (-10cm on Reachy's x axis):"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "56",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.r_arm.translate_by(-0.25, 0, 0, frame='robot')"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "57",
   "metadata": {},
   "source": [
    "You can easily make the gripper rotate with the `rotate_by(...)` method:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "58",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.r_arm.rotate_by(10, 0, 0, frame='gripper')"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "59",
   "metadata": {},
   "source": [
    "The gripper rotates by 10 degrees around the x axis of the gripper.  \n",
    "For both functions, you need to specify a frame:\n",
    "* setting **gripper** as frame will translate or rotate in the gripper coordinate system\n",
    "* setting **robot** as frame will translate or rotate directly in Reachy's coordinate system"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "60",
   "metadata": {},
   "source": [
    "If you want to compute a translation or rotation without making the robot move, you can call `get_translation_by(...)` and `get_rotation_by(...)` to get the corresponding pose 4x4 matrix:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "61",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.r_arm.get_translation_by(-0.1, 0.2, -0.1, frame='gripper')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "62",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.r_arm.get_rotation_by(-10, 20, 0, frame='gripper')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6d777afc",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.r_arm.goto_posture()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "63",
   "metadata": {},
   "source": [
    "## Gripper Control"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "64",
   "metadata": {},
   "source": [
    "Finally, you may want to open or close a gripper to grab an object! Use the `close` or `open` method to do so:\n",
    "> Those methods can't be set as blocking moves for now, so you need to add a waiting condition while the gripper is still moving. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "65",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.l_arm.gripper.close()\n",
    "while reachy.l_arm.gripper.is_moving():\n",
    "    time.sleep(0.1)\n",
    "reachy.l_arm.gripper.open()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "66",
   "metadata": {},
   "source": [
    "Of course, you can partially open the gripper, and get its current opening:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "67",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.l_arm.gripper.set_opening(55)\n",
    "while reachy.l_arm.gripper.is_moving():\n",
    "    time.sleep(0.1)\n",
    "reachy.l_arm.gripper.get_current_opening()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6c101c4d",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.r_arm.gripper.goto(100, duration=2, interpolation_mode=\"minimum_jerk\")\n",
    "reachy.r_arm.gripper.goto(0, duration=2, interpolation_mode=\"minimum_jerk\")\n",
    "reachy.r_arm.gripper.goto(100, duration=2, interpolation_mode=\"linear\")\n",
    "reachy.r_arm.gripper.goto(0, duration=2, interpolation_mode=\"linear\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "68",
   "metadata": {},
   "source": [
    "# Set robot back to compliant mode"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "69",
   "metadata": {},
   "outputs": [],
   "source": [
    "reachy.r_arm.gripper.open()\n",
    "reachy.goto_posture('default', duration=2, wait=True)\n",
    "    \n",
    "reachy.turn_off_smoothly()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
